//package com.hitqz.robot.dispatch.netty.strategy.publicPathStrategy.util;
//
//import cn.hutool.core.collection.CollectionUtil;
//import cn.hutool.core.util.StrUtil;
//import cn.hutool.extra.spring.SpringUtil;
//import com.alibaba.nacos.shaded.com.google.common.collect.Maps;
//import com.baomidou.mybatisplus.core.toolkit.Wrappers;
//import com.google.common.cache.Cache;
//import com.google.common.collect.HashBasedTable;
//import com.google.common.collect.Table;
//import com.hitqz.robot.api.business.dto.CmdRobot2PosDto;
//import com.hitqz.robot.api.business.dto.RobotClientSummaryDto;
//import com.hitqz.robot.api.business.entity.RobotEntity;
//import com.hitqz.robot.api.business.entity.RobotGroupEntity;
//import com.hitqz.robot.api.business.entity.RobotMapPosEntity;
//import com.hitqz.robot.api.business.enums.WorkStatusEnum;
//import com.hitqz.robot.api.dispatch.protocol.dto.RobotApplyPathDto;
//import com.hitqz.robot.api.dispatch.protocol.dto.RobotCmdNavDto;
//import com.hitqz.robot.api.dispatch.protocol.dto.RobotMapPosDto;
//import com.hitqz.robot.biz.service.RobotMapPosService;
//import com.hitqz.robot.biz.service.RobotService;
//import com.hitqz.robot.dispatch.netty.util.JtsUtil;
//import lombok.extern.slf4j.Slf4j;
//import org.locationtech.jts.geom.*;
//
//import java.util.*;
//import java.util.concurrent.*;
//import java.util.concurrent.atomic.AtomicBoolean;
//import java.util.stream.Collectors;
//
///**
// * @author xupkun
// * @date 2025/1/3
// */
//@Slf4j
//public class DynamicCalculationUtil {
//
//    private static ThreadPoolExecutor threadPoolExecutor= new ThreadPoolExecutor(
//            200,
//            200,
//            60,
//            TimeUnit.SECONDS,
//            new LinkedBlockingQueue<Runnable>(200),
//            Executors.defaultThreadFactory(),
//            new ThreadPoolExecutor.DiscardOldestPolicy());
//
//    private static String lockA = "path_list_lock";
//
//    private static String lockB = "path_list_action_lock";
//
//    private static String lockC = "move_other_path_lock";
//
//
//    /**
//     * key 机器人编码
//     * value 目标位置
//     */
//    private static volatile Map<String, RobotMapPosDto> ROBOT_TARGET_POS = Maps.newHashMap();
//    /**
//     * 更换机器人目标位置--中间过程点/ 记录的是如果前往中间点，一开始提交的最终目标点
//     * key 机器人编码
//     * value 目标位置
//     */
//    private static volatile Map<String, RobotMapPosDto> ROBOT_FINAL_POS = Maps.newHashMap();
//
//    /**
//     * key clientID
//     * value 中间点code
//     */
//    private static volatile Map<String, String> ROBOT_MIDDLE_POS = Maps.newHashMap();
//
//    /**
//     * key 机器人编码
//     * boolean true 暂停导航
//     * 是否暂停
//     */
//    private static  volatile Map<String, Boolean> ROBOT_STOP_FLAG = Maps.newHashMap();
//
//    /**
//     * 是否已经发送过点位
//     */
//    private static volatile Map<String, Boolean> ROBOT_SEND_POS_FLAG = Maps.newHashMap();
//
//    /**
//     * key 机器人编码
//     * value 路径列表
//     * 机器人路径列表
//     */
//    private static volatile Map<String, LinkedList<RobotMapPosDto>> ROBOT_PATH_LIST = Maps.newHashMap();
//
//    /**
//     * 公共路径数据
//     */
//    private static volatile Map<String, Boolean> ROBOT_BLOCK_ROBOT = Maps.newHashMap();
//
//    /**
//     *  避让 主机器人-》 避让的机器人-〉 避让的点位
//     */
//    public static Table<String, String, RobotMapPosDto> ROBOT_LOCK_TARGET_POS = HashBasedTable.create();
//
//
//
//
//
//    private static Table<String, String, List<RobotMapPosDto >> ROBOT_COMMON_PATH = HashBasedTable.create();
//
//
//    private static volatile Map<String, Boolean> AVOID_ROBOT = Maps.newConcurrentMap();
//
//    /**
//     * 计算策略---动态切换路线
//     * @param groupId
//     */
//    public static void dispatchMonitor(Integer groupId, List<RobotEntity> robotEntityList) {
//        robotEntityList.forEach(t-> {
//            if (CollectionUtil.isNotEmpty(ROBOT_PATH_LIST.get(t.getRobotSn()))) {
//                // 判断导航状态
//                boolean hasBlock = checkOtherCarBlock(groupId, t.getRobotSn(), null);
//                if (hasBlock) {
//                    // 暂停导航
//                    if(!ROBOT_STOP_FLAG.get(t.getRobotSn()) || ROBOT_STOP_FLAG.get(t.getRobotSn()) == null) {
//                        log.info("{} dispatchMonitor:暂停导航: {}-- {}", t.getRobotSn(), !ROBOT_STOP_FLAG.get(t.getRobotSn()), ROBOT_STOP_FLAG.get(t.getRobotSn()) == null);
//                        ROBOT_STOP_FLAG.put(t.getRobotSn(), true);
//                        changeNavStatus(t.getRobotSn(), 1);
//                    }
//                } else {
//                    if (ROBOT_STOP_FLAG.get(t.getRobotSn())) {
//                        ROBOT_STOP_FLAG.put(t.getRobotSn(), false);
//                        changeNavStatus(t.getRobotSn(), 2);
//                        log.info("下发继续执行任务" + t.getRobotSn());
//                    }
//                }
//            }
//        });
//    }
//
//    /**
//     * 提交路径
//     * @param group
//     * @param robotApplyPathDto
//     */
//    public static void submitPath(RobotGroupEntity group, RobotApplyPathDto robotApplyPathDto) {
//        // 提交路径
//        if (robotApplyPathDto.getPathList().size() == 0) {
//            //log.info("{} 提交的没有路径,清除数据 ",  robotApplyPathDto.getClientId());
//            clearRobot(robotApplyPathDto.getClientId());
//            return;
//        }
//        clearRobot(robotApplyPathDto.getClientId());
//        ROBOT_PATH_LIST.remove(robotApplyPathDto.getClientId());
//        LinkedList<RobotMapPosDto> pathList = robotApplyPathDto.getPathList();
//        String robotSn = robotApplyPathDto.getClientId();
//        ROBOT_SEND_POS_FLAG.put(robotSn, false);
//        // 最后要前往的终点，可能存在中间点，但是不影响最后的目标点
//        ROBOT_FINAL_POS.put(robotSn, robotApplyPathDto.getPathList().get(robotApplyPathDto.getPathList().size() -1));
//        // 计算能不能运动
//        boolean hasBlock = checkOtherCarBlock(group.getId(), robotSn, robotApplyPathDto.getPathList());
//        while (hasBlock) {
//            //log.info("{} 机器人被其他车占用,不下发点位", robotApplyPathDto.getClientId());
//            hasBlock = checkOtherCarBlock(group.getId(), robotSn, robotApplyPathDto.getPathList());
//        }
//        log.info("{} 机器人可以正常执行任务 {}", robotApplyPathDto.getClientId(), hasBlock);
//        // 设置路径
//
//        //ROBOT_TARGET_POS.put(robotSn, pathList.get(pathList.size()-1));
//        setTargetPos(robotSn, pathList.get(pathList.size()-1));
//        log.info("锁定目标成功: {}-- {}", robotApplyPathDto.getClientId(), pathList.get(pathList.size()-1).getCode());
//        DynamicCalculationUtil.getOrSetPathList(robotApplyPathDto.getClientId(), robotApplyPathDto.getPathList(), true);
//        //可以前往目标点后，锁定目标点，然后进行计算
//        ROBOT_SEND_POS_FLAG.put(robotSn, true);
//        // 继续执行任务，提示可以前往目标点
//        goFinalPos(robotSn, robotApplyPathDto.getPathList().get(robotApplyPathDto.getPathList().size()-1));
//    }
//
//
//    public static void setTargetPos(String robotSn, RobotMapPosDto robotMapPosDto) {
//        synchronized (robotSn.intern()) {
//            log.info("{} 设置目标点 {}", robotSn, robotMapPosDto.getCode());
//            ROBOT_TARGET_POS.put(robotSn, robotMapPosDto);
//        }
//    }
//
//    /**
//     * 目标点是否占用/路径上是否被其他车堵住
//     * @param mainClientId
//     * @return
//     */
//    public static boolean checkOtherCarBlock(Integer groupId, String mainClientId,  LinkedList<RobotMapPosDto> mainClientPath) {
//        //boolean isBlock = false;
//        String blockRobot = "";
//        AtomicBoolean isBlock = new AtomicBoolean(false);
//        if (mainClientPath == null) {
//            mainClientPath = ROBOT_PATH_LIST.get(mainClientId);
//        }
//        Cache<Integer, List<RobotEntity>> posInfoDtoCache = SpringUtil.getBean("robotDispatch");
//        if (posInfoDtoCache.getIfPresent(groupId) != null) {
//            List<RobotEntity> robotEntities = posInfoDtoCache.getIfPresent(groupId);
//            assert robotEntities != null;
//            Iterator<RobotEntity> iterator = robotEntities.iterator();
//            RobotMapPosDto currentTargetPos = mainClientPath.get(mainClientPath.size() -1);
//            Cache<String, RobotClientSummaryDto> robotClientSummaryDtoCache = SpringUtil.getBean("robotCache");
//            while (iterator.hasNext()) {
//                // 路径路径上上面是否存在车进行阻挡
//                RobotEntity otherRobot = iterator.next();
//                RobotClientSummaryDto robotClientSummaryDto = robotClientSummaryDtoCache.getIfPresent(otherRobot.getRobotSn());
//                try {
//                    assert robotClientSummaryDto != null;
//                    if (Objects.requireNonNull(robotClientSummaryDto).getLogicOnlineStatus() == 0) {
//                        // 调度下线的车不用循环管
//                        continue;
//                    }
//                    if (!otherRobot.getRobotSn().trim().equalsIgnoreCase(mainClientId.trim())) {
//                        // 计算目标点是否已经被其他车锁定
//                        try {
//                            RobotMapPosDto robotMapPosDto = ROBOT_TARGET_POS.get(otherRobot.getRobotSn());
//                            if (robotMapPosDto != null && currentTargetPos != null &&
//                                    currentTargetPos.getCode().equals(robotMapPosDto.getCode())) {
//                                blockRobot = otherRobot.getRobotSn();
//                                //log.info("{} 目标点被 {} 机器人 {} 占用", robotMapPosDto.getCode(), mainClientId, otherRobot.getRobotSn());
//                                isBlock.set(true);
//                                break;
//                            }
//                        } catch (Exception e) {
//                            log.info("{} 进行计算目标点是否被占用发生异常 {}", otherRobot.getRobotSn(), e);
//                        }
//
//                        // 计算其他的机器人的位置，是否有阻挡在路径上面
//                        try {
//                            if (CollectionUtil.isNotEmpty(mainClientPath)) {
//                                for (int i = 0; i < mainClientPath.size() - 1; i++) {
//                                    RobotMapPosDto start = mainClientPath.get(i);
//                                    RobotMapPosDto end = mainClientPath.get(i + 1);
//                                    Cache<String, Polygon> robotPosPolygon = SpringUtil.getBean("robotPosPolygon");
//                                    Polygon polygon = robotPosPolygon.getIfPresent(otherRobot.getRobotSn());
//                                    LineString lineSegment = JtsUtil.createLineString(start.getPosX(), start.getPosY(),
//                                            end.getPosX(), end.getPosY());
//                                    if (lineSegment.intersects(polygon)) {
//                                        blockRobot = otherRobot.getRobotSn();
//                                        //log.info("{} 有车在路上挡住需要停止： {} --- {}---{}", mainClientId, blockRobot, start.getCode(), end.getCode());
//                                        isBlock.set(true);
//                                        break;
//                                    }
//                                }
//                            }
//                        } catch (Exception e) {
//                            log.info("{} 进行计算其他车是否被占用发生异常 {}", otherRobot.getRobotSn(), e);
//                        }
//                        try {
//                            // 计算是不是存在公共路径
//                            // 可能是任务中，判断公共路径状态，然后判断公共路径以及方向，同向的话-如果距离太近的话，需要暂停
//                            if(!isBlock.get()) {
//                                LinkedList<RobotMapPosDto> blockRobotPath = ROBOT_PATH_LIST.get(otherRobot.getRobotSn());
//                                // 计算重叠的路径 2. 计算方向 3. 计算位置相对距离， <5 主动停止
//                                if (CollectionUtil.isNotEmpty(blockRobotPath) && blockRobotPath.size() > 1){
//                                    //log.info("{}、{} 的路径是 {}", mainClientId, otherRobot.getRobotSn(),
//                                    //        blockRobotPath.stream().map(RobotMapPosDto::getCode).collect(Collectors.joining("|")));
//                                    List<LineString> overlappingSegments = JtsUtil.findOverlappingSegments(mainClientPath, blockRobotPath);
//                                    List<RobotMapPosDto> samePath = JtsUtil.convertOverlappingSegmentsToPath(blockRobotPath, overlappingSegments);
//                                    String direction = JtsUtil.calculatePathDirection(mainClientPath, blockRobotPath);
//                                    //log.info("{}、{}运动的路径方向是 {}, 公共路径为: {}", mainClientId, otherRobot.getRobotSn(),
//                                    //        direction, samePath.stream().map(RobotMapPosDto::getCode).collect(Collectors.joining("|")));
//                                    ROBOT_COMMON_PATH.put(mainClientId, otherRobot.getRobotSn(), samePath);
//                                    if (CollectionUtil.isNotEmpty(samePath) && samePath.size() > 1) {
//                                        if (direction.equals("同向")) {
//                                            // 同向的话计算谁在前，谁在后， 如果距离过近，则暂停后面的车， 不然的话，同向是可以允许运行
//                                            robotClientSummaryDto = robotClientSummaryDtoCache.getIfPresent(mainClientId);
//                                            RobotClientSummaryDto blockClientSummaryDto = robotClientSummaryDtoCache.getIfPresent(otherRobot.getRobotSn());
//                                            assert robotClientSummaryDto != null;
//                                            assert blockClientSummaryDto != null;
//                                            int result = JtsUtil.calculateRelativePositionOnSamePath(
//                                                    new LinkedList<>(samePath), robotClientSummaryDto.getPosition().getLaserPos().getX(),
//                                                    robotClientSummaryDto.getPosition().getLaserPos().getY(),
//                                                    blockClientSummaryDto.getPosition().getLaserPos().getX(),
//                                                    blockClientSummaryDto.getPosition().getLaserPos().getY()
//                                            );
//                                            // 通行的车前面，阻塞的车在  只要知道后面的车就行。
//                                            // 计算距离
//                                            Cache<String, Polygon> robotPosPolygon = SpringUtil.getBean("robotPosPolygon");
//                                            double distance = Objects.requireNonNull(robotPosPolygon.getIfPresent(mainClientId)).
//                                                    distance(robotPosPolygon.getIfPresent(otherRobot.getRobotSn()));
//                                            // 后面的车与前面的车距离 7米，考虑暂停
//                                            //log.info("{}、{} 公共路径长度为 {}---{}", mainClientId, otherRobot.getRobotSn(), result, distance);
//                                            if (result == -1 ) {
//                                                if (distance < 7) {
//                                                    // 计算相对距离，如果距离在于一定的距离内，则暂停
//                                                    //log.info("{}、{} 路径方向相同距离过近 {} ，暂停", mainClientId, otherRobot.getRobotSn(), distance);
//                                                    //isBlock.set(true);
//                                                    break;
//                                                } else {
//
//                                                }
//                                            } else {
//                                                //isBlock.set(true);
//                                            }
//                                        } else {
//                                            // 反向或者是其他的情况
//                                            RobotMapPosDto finalCode = ROBOT_TARGET_POS.get(blockRobot);
//                                            if (finalCode == null) {
//                                                log.info("{} 路径为{}、{} 路径为{} 运动的路径方向是 {}, 公共路径为: {}", mainClientId,
//                                                        mainClientPath.stream().map(RobotMapPosDto::getCode).collect(Collectors.joining("|")),
//                                                        otherRobot.getRobotSn(), blockRobotPath.stream().map(RobotMapPosDto::getCode).collect(Collectors.joining("|")),
//                                                        direction, samePath.stream().map(RobotMapPosDto::getCode).collect(Collectors.joining("|")));
//                                                isBlock.set(true);
//                                                break;
//                                            }
//                                        }
//                                    }
//                                }
//                            }
//                        } catch (Exception e) {
//                            log.info("计算距离发生异常:" + e);
//                        }
//                        try {
//                            // 计算是不是存在公共路径 -- 计算相较的车的距离。距离小于7的话就要停止
//                            Cache<String, Polygon> robotPosPolygon = SpringUtil.getBean("robotPosPolygon");
//                            double distance = Objects.requireNonNull(robotPosPolygon.getIfPresent(mainClientId)).
//                                    distance(robotPosPolygon.getIfPresent(otherRobot.getRobotSn()));
//                            // 后面的车与前面的车距离 7米，考虑暂停
//                            //log.info("{}、{} 公共路径长度为 {}---{}", mainClientId, otherRobot.getRobotSn(), result, distance);
//                            RobotClientSummaryDto otherClientSummaryDto = robotClientSummaryDtoCache.getIfPresent(otherRobot.getRobotSn());
//                            //log.info("");
//                            //if (distance < 4.0 &&
//                            //        Objects.requireNonNull(otherClientSummaryDto).getStatus().getWorkStatus() == Integer.parseInt(WorkStatusEnum.WORK.getCode())) {
//                            //    isBlock.set(true);
//                            //    break;
//                            //} else {
//                            //    break;
//                            //}
//                        } catch (Exception e) {
//                            log.info("计算距离发生异常:" + e);
//                        }
//                    }
//                } catch (Exception e) {
//                    log.info("判断其他车的状态异常：{}", e);
//                }
//            }
//        }
//        // 查看路径上面是不是有可能存在路径上的点被其他车占用
//        // 判断是否静止，驱动堵塞的车先去避让
//        if (isBlock.get()) {
//            String finalBlockRobot = blockRobot;
//            LinkedList<RobotMapPosDto> finalMainClientPath = mainClientPath;
//            String finalBlockRobot1 = blockRobot;
//            threadPoolExecutor.submit(()-> {
//                synchronized (lockC.intern()) {
//                    if (StrUtil.isEmpty(ROBOT_MIDDLE_POS.get(finalBlockRobot))) {
//                        Cache<String, RobotClientSummaryDto> robotClientSummaryDtoCache = SpringUtil.getBean("robotCache");
//                        RobotClientSummaryDto robotClientSummaryDto = robotClientSummaryDtoCache.getIfPresent(finalBlockRobot);
//                        if (robotClientSummaryDto != null) {
//                            if (robotClientSummaryDto.getLogicOnlineStatus() == 0) {
//                                // 有一台是逻辑(调度)下线的，存在不可预知的问题，进行报警，通知人工进行处理
//                                //log.info("阻挡的车是调度下线,处于不可控制状态");
//                            } else {
//                                // 如果是逻辑(调度)上线，只要是非任务状态就控制驱赶去其他位置，表示可以控制， 然后控制驱动
//                                // if (Double.parseDouble(robotClientSummaryDto.getStatus().getSpeed()) == 0) {
//                                // 空闲/任务中，等待路径释放
//                                // 目标点位为空，代表的是可能只是放置
//                                RobotMapPosDto blockRobotTargetPos = ROBOT_TARGET_POS.get(finalBlockRobot);
//                                if ((robotClientSummaryDto.getStatus().getWorkStatus() == Integer.parseInt(WorkStatusEnum.FREE.getCode()))
//                                        || (Double.parseDouble(robotClientSummaryDto.getStatus().getSpeed()) == 0
//                                        && robotClientSummaryDto.getStatus().getWorkStatus() == Integer.parseInt(WorkStatusEnum.WORK.getCode())
//                                )
//                                ) {
//                                    // 阻挡的AGV/机器人处于空闲中，直接前往其他空闲的点进行等待
//                                    //log.info("速度为0驱动其他车前往其他点位置，查询最近的处于路径之外的点{} ", finalBlockRobot);
//                                    // 1. 获取全部的点
//                                    // 2. 获取从缓存中获取车当前的位置
//                                    // 3. 过滤得到路径外面的点
//                                    // 4. 计算当前点到路径的距离，然后取最近的点，然后进行导航
//                                    String code = findNearestPointOutsidePath(mainClientId, finalBlockRobot, finalMainClientPath);
//                                    if(StrUtil.isNotBlank(code) && (AVOID_ROBOT.get(finalBlockRobot) == null || !AVOID_ROBOT.get(finalBlockRobot))) {
//                                        log.info("{}驱动AGV前往{}---执行的AGV是{}----目标点{} ",mainClientId, code, finalBlockRobot, blockRobotTargetPos);
//                                        AVOID_ROBOT.put(finalBlockRobot, true);
//                                        ROBOT_MIDDLE_POS.put(finalBlockRobot, code);
//                                        changeMiddlePos(groupId, mainClientId, finalBlockRobot, code, finalMainClientPath.get(finalMainClientPath.size() -1) );
//                                        isBlock.set(true);
//                                    }
//                                }  else {
//                                    // 可能是任务中，判断公共路径状态，然后判断公共路径以及方向，同向的话-如果距离太近的话，需要暂停
//                                    LinkedList<RobotMapPosDto> blockRobotPath = ROBOT_PATH_LIST.get(finalBlockRobot1);
//                                    // 计算重叠的路径 2. 计算方向 3. 计算位置相对距离， <5 主动停止
//                                    List<LineString> overlappingSegments = JtsUtil.findOverlappingSegments(finalMainClientPath, blockRobotPath);
//                                    List<RobotMapPosDto> samePath = JtsUtil.convertOverlappingSegmentsToPath(blockRobotPath, overlappingSegments);
//                                    String direction = JtsUtil.calculatePathDirection(finalMainClientPath, blockRobotPath);
//                                    //log.info("{}、{}运动的路径方向是 {}, 公共路径为: {}", mainClientId, finalBlockRobot,
//                                    //        direction, samePath.stream().map(RobotMapPosDto::getCode).collect(Collectors.joining("|")));
//                                    if (CollectionUtil.isNotEmpty(samePath) && direction.equals("同向")) {
//                                        // 同向的话计算谁在前，谁在后， 如果距离过近，则暂停后面的车， 不然的话，同向是可以允许运行
//                                        robotClientSummaryDto = robotClientSummaryDtoCache.getIfPresent(finalBlockRobot);
//                                        RobotClientSummaryDto blockClientSummaryDto = robotClientSummaryDtoCache.getIfPresent(finalBlockRobot);
//                                        assert robotClientSummaryDto != null;
//                                        assert blockClientSummaryDto != null;
//                                        int result = JtsUtil.calculateRelativePositionOnSamePath(
//                                                new LinkedList<>(samePath), robotClientSummaryDto.getPosition().getLaserPos().getX(),
//                                                robotClientSummaryDto.getPosition().getLaserPos().getY(),
//                                                blockClientSummaryDto.getPosition().getLaserPos().getX(),
//                                                blockClientSummaryDto.getPosition().getLaserPos().getY()
//                                        );
//                                        // 通行的车前面，阻塞的车在  只要知道后面的车就行。
//                                        // 计算距离
//                                        //Cache<String, Polygon> robotPosPolygon = SpringUtil.getBean("robotPosPolygon");
//                                        //double distance = Objects.requireNonNull(robotPosPolygon.getIfPresent(mainClientId)).
//                                        //        distance(robotPosPolygon.getIfPresent(finalBlockRobot2));
//                                        //// 后面的车与前面的车距离 7米，考虑暂停
//                                        //if (result == -1 && distance < 7) {
//                                        //    // 计算相对距离，如果距离在于一定的距离内，则暂停
//                                        //    //log.info("{}、{} 路径方向相同距离过近 {} ，暂停", mainClientId, finalBlockRobot, distance);
//                                        //    isBlock.set(true);
//                                        //} else {
//                                        //    //log.info("{}、{} 路径方向相同距离大于一定距离 {}，认为不是阻挡", mainClientId, finalBlockRobot, distance);
//                                        //    isBlock.set(false);
//                                        //}
//                                    }
//                                }
//                            }
//                        }
//                    } else {
//                        // 不为空的时候，计算公共路径数据
//
//                    }
//                }
//            });
//        }
//
//        ROBOT_BLOCK_ROBOT.put(mainClientId, isBlock.get());
//        return isBlock.get();
//    }
//
//
//    public static void getOrSetPathList(String robotSn, LinkedList<RobotMapPosDto> robotPathList,Boolean setFlag) {
//        synchronized (lockA) {
//            if (setFlag) {
//                log.info("{} 提交的机器人路径:{}", robotSn, robotPathList.stream().map(RobotMapPosDto::getCode).collect(Collectors.joining("|")));
//                ROBOT_PATH_LIST.put(robotSn, robotPathList);
//            } else {
//                List<RobotMapPosDto> currentPathList = ROBOT_PATH_LIST.get(robotSn);
//                if (currentPathList == null || currentPathList.isEmpty()) {
//                    return;
//                }
//                // 到达点位
//                Cache<String, RobotClientSummaryDto> posInfoDtoCache = SpringUtil.getBean("robotCache");
//                if (posInfoDtoCache == null) {
//                    return;
//                }
//                try {
//                    RobotClientSummaryDto robotClientSummaryDto = posInfoDtoCache.getIfPresent(robotSn);
//                    if (robotClientSummaryDto == null) {
//                        return;
//                    }
//                    String nearStation = robotClientSummaryDto.getPosition().getLaserPos().getNearPosCode();
//                    String lastArrivePos = robotClientSummaryDto.getPosition().getLaserPos().getLastPosCode();
//                    if (StrUtil.isEmpty(nearStation) && StrUtil.isEmpty(lastArrivePos)) {
//                        return;
//                    }
//                    int pathListSize = currentPathList.size();
//                    int arrivePosIndex = -1;
//                    for (int i = 0; i < pathListSize; i++) {
//                        if (lastArrivePos.equalsIgnoreCase(currentPathList.get(i).getCode())) {
//                            arrivePosIndex = i;
//                            break;
//                        }
//                    }
//
//                    if (arrivePosIndex == -1) {
//                        return;
//                    }
//                    // 增加同步控制
//                    synchronized (lockB) {
//                        currentPathList.remove(arrivePosIndex);
//                        ROBOT_PATH_LIST.put(robotSn, new LinkedList<>(currentPathList));
//                        if (currentPathList.isEmpty()) {
//                            //log.info("已经执行完路径，清空所有数据" + robotSn);
//                            RobotMapPosDto finalCode = ROBOT_FINAL_POS.get(robotSn);
//                            String middlePos = ROBOT_MIDDLE_POS.get(robotSn);
//                            if (StrUtil.isNotEmpty(middlePos)) {
//                                //log.info("可能是驱赶其他车辆，然后前往中间点，继续计算能不能前往目标点{}---{}", finalCode.getCode(), middlePos);
//                                //ROBOT_MIDDLE_POS.remove(robotSn);
//                            } else {
//                                //log.info("可能是任务执行完成了{}---{}", finalCode.getCode(), middlePos);
//                                ROBOT_PATH_LIST.put(robotSn, new LinkedList<>());
//                            }
//                            clearRobot(robotSn);
//                            //log.info("清空后的路径数据"+ ROBOT_PATH_LIST.get(robotSn).size());
//                        }
//                    }
//
//                } catch (Exception e) {
//                    log.error("Error processing robot path: {}", e.getMessage(), e);
//                }
//            }
//        }
//    }
//
//
//    /**
//     * 清空数据
//     * @param robotSn
//     */
//    public static void clearRobot(String robotSn) {
//        ROBOT_TARGET_POS.put(robotSn, null);
//        ROBOT_FINAL_POS.put(robotSn, null);
//        ROBOT_PATH_LIST.put(robotSn, new LinkedList<>());
//    }
//
//
//    /**
//     * 修改中间点
//     * @param clientId
//     */
//    public static void changeMiddlePos(Integer groupId, String mainClientId, String clientId, String code, RobotMapPosDto finalCode) {
//        // 更换目标
//        CmdRobot2PosDto dto = new CmdRobot2PosDto();
//        dto.setClientId(clientId);
//        dto.setPosCode(code);
//        SpringUtil.getApplicationContext().publishEvent(dto);
//        log.info("{} 下发中间的点之后，在前往目标点 {}", clientId, code);
//        CompletableFuture.runAsync(()-> {
//            while (true) {
//                try {
//                    Cache<String, RobotClientSummaryDto> robotClientSummaryDtoCache = SpringUtil.getBean("robotCache");
//                    RobotClientSummaryDto blockRobotSummaryDto = robotClientSummaryDtoCache.getIfPresent(clientId);
//                    if (blockRobotSummaryDto.getPosition().getLaserPos().getLastPosCode().equals(code)) {
//                        Thread.sleep(5000);
//                        RobotMapPosDto goTargetPos = ROBOT_TARGET_POS.get(clientId);
//                        if (goTargetPos != null) {
//                            log.info("{} 等待中间点到达 {} ", clientId, code);
//                            ROBOT_MIDDLE_POS.remove(goTargetPos);
//                            while (!checkOtherCarBlock(groupId, clientId, null)
//                                    && (ROBOT_BLOCK_ROBOT.get(mainClientId) != null && !(ROBOT_BLOCK_ROBOT.get(mainClientId)))) {
//                                log.info("中间点的过程中可以继续往后面走了 {}--{}", clientId, goTargetPos.getCode());
//                                AVOID_ROBOT.put(clientId, false);
//                                goFinalPos(clientId, goTargetPos);
//                                break;
//                            }
//                            break;
//                        } else {
//                            log.info("中间点已经到达，但是目标点已经不存在了，直接结束 {}--- {}", clientId, code);
//                            break;
//                        }
//                    }
//                } catch (Exception e) {
//
//                }
//            }
//        });
//    }
//
//
//    /**
//     * 修改中间点
//     * @param clientId
//     */
//    public static void goFinalPos(String clientId, RobotMapPosDto finalPos) {
//        // 更换目标
//        CmdRobot2PosDto dto = new CmdRobot2PosDto();
//        dto.setClientId(clientId);
//        dto.setPosCode(finalPos.getCode());
//        SpringUtil.getApplicationContext().publishEvent(dto);
//        log.info("{} 下发可以前往目标站点:{}",clientId, finalPos.getCode());
//    }
//
//
//    /**
//     * 修改导航状态
//     * @param clientId
//     * @param status
//     */
//    public static void changeNavStatus(String clientId, int status) {
//        SpringUtil.getApplicationContext().publishEvent(new RobotCmdNavDto(clientId, status));
//    }
//
//
//
//    /**
//     * 查询其他车路径外距离车位置最近的点
//     * @param mainClientId
//     * @return 最近的路径外点
//     */
//    public static String findNearestPointOutsidePath(String mainClientId, String blockRobotId,LinkedList<RobotMapPosDto> mainPath ) {
//        String code = "";
//        // 判断路径是否为空，或者
//        if (CollectionUtil.isEmpty(mainPath)) {
//            return code;
//        }
//        // 获取当前车的位置
//        Cache<String, RobotClientSummaryDto> robotClientSummaryDtoCache = SpringUtil.getBean("robotCache");
//        Cache<String, Polygon> robotPosPolygon = SpringUtil.getBean("robotPosPolygon");
//        RobotClientSummaryDto mainRobotClientSummaryDto = robotClientSummaryDtoCache.getIfPresent(mainClientId);
//        if (mainRobotClientSummaryDto == null) {
//            return null;
//        }
//        RobotMapPosService robotMapPosService = SpringUtil.getBean(RobotMapPosService.class);
//        RobotService robotService = SpringUtil.getBean(RobotService.class);
//        RobotEntity robotEntity = robotService.selectByRobotSn(mainClientId);
//
//        List<RobotMapPosEntity> robotMapPosDtoList = robotMapPosService.list(Wrappers.<RobotMapPosEntity>lambdaQuery()
//                .eq(RobotMapPosEntity::getMapCode, robotEntity.getMapCode()));
//        if (CollectionUtil.isNotEmpty(robotMapPosDtoList)) {
//            List<RobotMapPosEntity> pointsOutsidePath = new ArrayList<>();
//            for (RobotMapPosEntity point : robotMapPosDtoList) {
//                if (!isPointOnPath(point, mainPath)) {
//                    pointsOutsidePath.add(point);
//                }
//            }
//            if (CollectionUtil.isNotEmpty(pointsOutsidePath)) {
//                double minDistance = Double.MAX_VALUE;
//                for (RobotMapPosEntity point : pointsOutsidePath) {
//                    // 位置可能会发生改变，比如遥控器控制
//                    RobotClientSummaryDto blockRobotSummaryDto = robotClientSummaryDtoCache.getIfPresent(blockRobotId);
//                    Polygon mainCar = robotPosPolygon.getIfPresent(mainClientId);
//                    assert blockRobotSummaryDto != null;
//                    double distance = calculateDistance(blockRobotSummaryDto.getPosition().getLaserPos().getX(),
//                            blockRobotSummaryDto.getPosition().getLaserPos().getY()
//                            , point.getPosX(), point.getPosY());
//                    if (distance < minDistance) {
//                        // 判断是不是与主车有相交，如果有则当作废弃
//                        Point gemPoint = JtsUtil.getPoint(point.getPosX(), point.getPosY());
//                        assert mainCar != null;
//                        if (!mainCar.intersects(gemPoint)) {
//                            RobotMapPosDto posCode = ROBOT_TARGET_POS.get(mainClientId);
//                            if (posCode != null &&  point.getCode().equals(posCode.getCode())) {
//                                minDistance = distance;
//                                code = point.getCode();
//                            }
//                            if (posCode == null) {
//                                minDistance = distance;
//                                code = point.getCode();
//                            }
//                        }
//
//                    }
//                }
//            }
//        }
//        return code;
//    }
//
//
//
//    /**
//     * 判断点是否在路径上
//     * @param point 待判断的点
//     * @param path 路径
//     * @return 如果点在路径上返回true，否则返回false
//     */
//    private static boolean isPointOnPath(RobotMapPosEntity point, LinkedList<RobotMapPosDto> path) {
//        if (CollectionUtil.isEmpty(path)) {
//            return false;
//        }
//        GeometryFactory geometryFactory = new GeometryFactory();
//        Coordinate pointCoord = new Coordinate(point.getPosX(), point.getPosY());
//        LineString pointLine = geometryFactory.createLineString(new Coordinate[]{pointCoord, pointCoord});
//        // 判断路径上每个线段与待判断点是否相交
//        for (int i = 0; i < path.size() - 1; i++) {
//            try {
//                RobotMapPosDto start = path.get(i);
//                RobotMapPosDto end = path.get(i + 1);
//                LineString pathSegment = JtsUtil.createLineString(start.getPosX(), start.getPosY(), end.getPosX(), end.getPosY());
//                if (pathSegment.intersects(pointLine)) {
//                    return true;
//                }
//            } catch (Exception e) {
//                log.info("计算是否在路径在上面:" + e.getMessage());
//                return false;
//            }
//
//        }
//        return false;
//    }
//
//    /**
//     * 计算两点之间的距离
//     * @param x1 第一个点的X坐标
//     * @param y1 第一个点的Y坐标
//     * @param x2 第二个点的X坐标
//     * @param y2 第二个点的Y坐标
//     * @return 两点之间的距离
//     */
//    private static double calculateDistance(double x1, double y1, double x2, double y2) {
//        return Math.sqrt(Math.pow(x2 - x1, 2) + Math.pow(y2 - y1, 2));
//    }
//
//
//
//
//
//}
//
//
